#include <iostream>
#include <cmath>
#include <list>
#include <fstream>
#include <string>
#include <boost/numeric/mtl/mtl.hpp>
#include <cppunit/extensions/HelperMacros.h>
#include "../src/fmm.h"
#include "../src/isoline.h"
#include "../src/backtracking.h"
#include "Test_backtracking.h"

CPPUNIT_TEST_SUITE_REGISTRATION(Test_backtracking);

void Test_backtracking::setUp()
{
}
void Test_backtracking::tearDown()
{
}

void Test_backtracking::test_LU_Solver_01()
{
	double data[][4]= {{2,3,4,5},
					{4,10,13,16},
					{6,25,38,46},
					{8,32,77,100}};
	unsigned size= 4;

	MATRIX  A(data);
	VECTOR  v(size);
	for (unsigned i = 0; i < size; ++i)
	{
		v[i] = SCALAR (i);
	}
	VECTOR w(A * v);

	VECTOR solution(size);

	CPPUNIT_ASSERT (true == Backtracking <double>::LU_Solver(A, w, solution));
	for (unsigned i = 0; i < size; ++i)
	{
		CPPUNIT_ASSERT(fabs(v[i] - solution[i])< ERROR);
	}
}

void Test_backtracking::test_LU_Solver_02()
{
	double data[][4]= {{1,1,1,1},
					{1,1,1,1},
					{6,25,38,46},
					{8,32,77,100}};
	unsigned size= 4;
	MATRIX	A(data);
	VECTOR	v(size);
	for (unsigned i = 0; i < size; ++i)
	{
		v[i] = SCALAR (i);
	}
	VECTOR w(A * v);
	VECTOR solution(size);

	CPPUNIT_ASSERT (false == Backtracking <double>::LU_Solver(A, w, solution));
}

void Test_backtracking::test_Heun_01()
{
	const unsigned size = 6;
	POINT vertex[size];
	vertex[0] = POINT(0,1,0);  //A
	vertex[1] = POINT(0,0,0);  //B
	vertex[2] = POINT(1,1,0);  //C
	vertex[3] = POINT(-1,1,0); //D
	vertex[4] = POINT(1,0,0);  //E
	vertex[5] = POINT(1,2,0);  //F

	//f(x,y) = a0 + a1 x + a2 y + a3 x^2 + a4 y ^2 + a5 xy
	double data[size][size];

	VECTOR	value(size);
	for (int i = 0; i < size; ++i)
	{
		data[i][0] = 1;
		data[i][1] = vertex[i][0];
		data[i][2] = vertex[i][1];
		data[i][3] = vertex[i][0] * vertex[i][0];
		data[i][4] = vertex[i][1] * vertex[i][1];
		data[i][5] = vertex[i][0] * vertex[i][1];
		value[i]   = i ;
	}
	MATRIX	Matrix(data);
	VECTOR coeff(size);
	CPPUNIT_ASSERT (true == Backtracking <double>::LU_Solver(Matrix, value, coeff));
	for (int i = 0; i < size; ++i)
	{
		double result = coeff[0] + coeff[1]* vertex[i][0] + coeff[2]* vertex[i][1] 
			+ coeff[3]* vertex[i][0]*vertex[i][0] + coeff[4]* vertex[i][1]*vertex[i][1]
			+ coeff[5]* vertex[i][0]*vertex[i][1];
		CPPUNIT_ASSERT ( std::fabs(value [i] - result) < ERROR );
	}
	POINT start (0,0,0);
	POINT p = Backtracking <double>::Huen (coeff, start, 0.01);
	POINT p_result (-0.0047f, 0.0341f, 0.0f);
//	std::cout<<"Result : "<<p <<std::endl;
	CPPUNIT_ASSERT ( (p - p_result).length() < ERROR);
}

void Test_backtracking::test_Heun_02()
{
	const unsigned size = 6;
	POINT vertex[size];
	vertex[0] = POINT(0,0,0);  //A
	vertex[1] = POINT(0.0181475,0,0);  //B
	vertex[2] = POINT(0.00685645,0.0131086,0);  //C
	vertex[3] = POINT(0.00399998, -0.00620554,0); //D
	vertex[4] = POINT(0.0233852, 0.011401, 0);  //E
	vertex[5] = POINT(0.00158343, 0.00684359, 0);  //F

	//f(x,y) = a0 + a1 x + a2 y + a3 x^2 + a4 y ^2 + a5 xy
	double data[size][size];

	VECTOR	value(size);
	value [0] = 0.928088;
	value [1] = 0.945683;
	value [2] = 0.938944;
	value [3] = 0.93055;
	value [4] = 0.953943;
	value [5] = 0.934158;

	for (int i = 0; i < size; ++i)
	{
		data[i][0] = 1;
		data[i][1] = vertex[i][0];
		data[i][2] = vertex[i][1];
		data[i][3] = vertex[i][0] * vertex[i][0];
		data[i][4] = vertex[i][1] * vertex[i][1];
		data[i][5] = vertex[i][0] * vertex[i][1];
	}
	MATRIX Matrix(data);
	VECTOR coeff(size);
	CPPUNIT_ASSERT (true == Backtracking <double>::LU_Solver(Matrix, value, coeff));
	for (int i = 0; i < size; ++i)
	{
		double result = coeff[0] + coeff[1]* vertex[i][0] + coeff[2]* vertex[i][1] 
			+ coeff[3]* vertex[i][0]*vertex[i][0] + coeff[4]* vertex[i][1]*vertex[i][1]
			+ coeff[5]* vertex[i][0]*vertex[i][1];
		CPPUNIT_ASSERT ( std::fabs(value [i] - result) < ERROR );
	}
	/*
	std::cout<<"Coefficient "<<std::endl;
	for (int i =0 ;i < 6; ++i)
	{
		std::cout<<coeff[i]<<std::endl;
	}
	*/
	POINT start (0.00938596, 0.00407247, 0);
	/*
	for (int i = 0; i < 100; ++i)
	{
		POINT p = Backtracking <double>::Huen (coeff, start, 0.001);
		std::cout<<"Result : "<<p <<std::endl;
		start = p;
	}
	*/
	//CPPUNIT_ASSERT ( (p - p_result).length() < ERROR);
}
void Test_backtracking::test_cross_product()
{
	VECTOR v1 (3);
	v1[0] = 1; v1[1] = 0; v1[2] = 0;
	VECTOR v2 (3);
	v2[0] = 0; v2[1] = 1; v2[2] = 0;
	VECTOR v3 = Backtracking<double>::cross_product(v1, v2);
	VECTOR v3_result (3);
	v3_result[0] = 0; v3_result[1] = 0; v3_result[2] = 1;
	VECTOR minus (v3 - v3_result);
	CPPUNIT_ASSERT ( mtl::vector::two_norm(minus) < ERROR);
}

void Test_backtracking::test_is_in_face_01()
{
	POINT vertex [3];
	vertex [0] = POINT (1,0,0);
	vertex [1] = POINT (0,1,0);
	vertex [2] = POINT (0,0,1);

	POINT p = POINT (1,1,1);
	int edge_idx; 
	//CPPUNIT_ASSERT ( false == Backtracking<double>::is_in_face (vertex, p, edge_idx));

	p = POINT (1.0/3.0, 1.0/3.0 , 1.0/3.0);
	//CPPUNIT_ASSERT ( true == Backtracking<double>::is_in_face (vertex, p, edge_idx));
	//CPPUNIT_ASSERT ( edge_idx == -1);

	p = POINT (5, 5, -9);
	//CPPUNIT_ASSERT ( true == Backtracking<double>::is_in_face (vertex, p, edge_idx));
	//std::cout<<edge_idx << std::endl;
	//CPPUNIT_ASSERT ( edge_idx == 0);
}
void Test_backtracking::test_is_in_face_02()
{
	POINT vertex [3];
	vertex [0] = POINT (1,0,0);
	vertex [2] = POINT (0,1,0);
	vertex [3] = POINT (0,0,0);

}
void Test_backtracking::test_map_sixpoint_to_plane_01()
{
	const unsigned size = 6;
	POINT vertex[size];
	vertex[0] = POINT(0,0,0);  //A
	vertex[1] = POINT(1,0,0);  //B
	vertex[2] = POINT(1,1,0);  //C
	vertex[3] = POINT(0,-1,0); //D
	vertex[4] = POINT(2,1,0);  //E
	vertex[5] = POINT(0,1,0);  //F

	POINT result [size];
	MATRIX rotation;
	VECTOR translation;
	Backtracking<double>::map_sixpoint_to_plane(vertex, result, rotation, translation);


	for (int i = 0;i < size; ++i)
	{
		CPPUNIT_ASSERT (abs(vertex[i][0] - result[i][0]) < ERROR);
		CPPUNIT_ASSERT (abs(vertex[i][1] - result[i][1]) < ERROR);
		CPPUNIT_ASSERT (abs(vertex[i][2] - result[i][2]) < ERROR);
	}
}

void Test_backtracking::test_map_sixpoint_to_plane_02()
{
	const unsigned size = 6;
	POINT vertex[size];
	vertex[0] = POINT(0,0,-1);  //A
	vertex[1] = POINT(0,1,-1);  //B
	vertex[2] = POINT(0,1,0);   //C
	vertex[3] = POINT(-1,0,-1); //D
	vertex[4] = POINT(-1,1,0);  //E
	vertex[5] = POINT(0,0,0);   //F

	POINT answer[size];
	answer[0] = POINT(0,0,0);  //A
	answer[1] = POINT(1,0,0);  //B
	answer[2] = POINT(1,1,0);  //C
	answer[3] = POINT(0,-1,0); //D
	answer[4] = POINT(2,1,0);  //E
	answer[5] = POINT(0,1,0);  //F

	POINT result [size];
	MATRIX rotation;
	VECTOR translation;
	Backtracking<double>::map_sixpoint_to_plane(vertex, result, rotation, translation);


	for (int i = 0;i < size; ++i)
	{
		CPPUNIT_ASSERT (abs(answer[i][0] - result[i][0]) < ERROR);
		CPPUNIT_ASSERT (abs(answer[i][1] - result[i][1]) < ERROR);
		CPPUNIT_ASSERT (abs(answer[i][2] - result[i][2]) < ERROR);
	}
}
void Test_backtracking::test_map_threepoint_to_plane_01()
{
}
void Test_backtracking::test_map_threepoint_to_plane_02()
{
}
void Test_backtracking::test_convert_coordinate_01()
{
	POINT tri_A[3], tri_B[3];
	tri_A[0] = POINT (0,0,0);
	tri_A[1] = POINT (0,1,0);
	tri_A[2] = POINT (1,1,0);

	tri_B[0] = POINT (0,0,0);
	tri_B[1] = POINT (0,1,0);
	tri_B[2] = POINT (1,1,0);

	MATRIX rotation;
	VECTOR translation;
	Backtracking<double>::convert_coordinate(tri_A, tri_B, rotation, translation);

	POINT result[3];

	for (int i = 0; i < 3; ++i)
	{
		VECTOR v(3), res(3);
		v = Backtracking<double>::point2vector(tri_A[i]);
		v = v + translation;
		res = rotation * v;
		result[i] = Backtracking<double>::vector2point(res);
		CPPUNIT_ASSERT (abs (result[i][0] - tri_B[i][0]) < ERROR);
		CPPUNIT_ASSERT (abs (result[i][1] - tri_B[i][1]) < ERROR);
		CPPUNIT_ASSERT (abs (result[i][2] - tri_B[i][2]) < ERROR);
	}
}

void Test_backtracking::test_convert_coordinate_02()
{
	POINT tri_A[3], tri_B[3];
	tri_A[0] = POINT (0,0,0);
	tri_A[1] = POINT (0,1,0);
	tri_A[2] = POINT (1,1,0);

	tri_B[0] = POINT (0,0,0);
	tri_B[1] = POINT (0,0,1);
	tri_B[2] = POINT (0,1,1);

	MATRIX rotation;
	VECTOR translation;
	Backtracking<double>::convert_coordinate(tri_A, tri_B, rotation, translation);

	POINT result[3];

//	std::cout<<rotation<<std::endl;
//	std::cout<<translation<<std::endl;
	for (int i = 0; i < 3; ++i)
	{
		VECTOR v(3), res(3);
		v = Backtracking<double>::point2vector(tri_A[i]);
		v = v + translation;
		res = rotation * v;
		result[i] = Backtracking<double>::vector2point(res);
		CPPUNIT_ASSERT (abs (result[i][0] - tri_B[i][0]) < ERROR);
		CPPUNIT_ASSERT (abs (result[i][1] - tri_B[i][1]) < ERROR);
		CPPUNIT_ASSERT (abs (result[i][2] - tri_B[i][2]) < ERROR);
	}
}

void Test_backtracking::test_convert_coordinate_03()
{
	//B is the flipped one; 
	POINT tri_A[3], tri_B[3];
	tri_A[0] = POINT (5,2,3);
	tri_A[1] = POINT (7,4,9);
	tri_A[2] = POINT (9,8,27);
	
	MATRIX rotation;
	VECTOR translation; 
	Backtracking<double>::map_threepoint_to_plane(tri_A, tri_B, rotation, translation);
	//Backtracking<double>::convert_coordinate(tri_A, tri_B, rotation, translation);
	POINT result[3];
	for (int i = 0;i < 3; ++i)
	{
		Backtracking<double>::convert_A2B(tri_A[i], rotation, translation, result[i]);
		CPPUNIT_ASSERT(abs(tri_B[i][0] - result[i][0]) < ERROR );
		CPPUNIT_ASSERT(abs(tri_B[i][1] - result[i][1]) < ERROR );
		CPPUNIT_ASSERT(abs(tri_B[i][2] - result[i][2]) < ERROR );
	}
/*
	std::cout<<std::endl;

	for (int i = 0; i< 3; ++i)
	{
		std::cout<<tri_A[i]<<' '<<tri_B[i]<<std::endl; 
	}
*/
	for (int i = 0; i< 3; ++i)
	{
		Backtracking<double>::convert_B2A(tri_B[i], rotation, translation, result [i]);
//		std::cout<<"From "<<tri_B[i]<<" to "<<result[i]<<std::endl;
		CPPUNIT_ASSERT(abs(tri_A[i][0] - result[i][0]) < ERROR );
		CPPUNIT_ASSERT(abs(tri_A[i][1] - result[i][1]) < ERROR );
		CPPUNIT_ASSERT(abs(tri_A[i][2] - result[i][2]) < ERROR );

	}

}

void Test_backtracking::test_convert_coordinate_04()
{
	//B is the flipped one; 
	POINT tri_A[3], tri_B[3];
	tri_A[0] = POINT (1,2,3);
	tri_A[1] = POINT (1,4,9);
	tri_A[2] = POINT (1,8,27);

	MATRIX rotation;
	VECTOR translation; 
	Backtracking<double>::map_threepoint_to_plane(tri_A, tri_B, rotation, translation);
	//Backtracking<double>::convert_coordinate(tri_A, tri_B, rotation, translation);
	POINT result[3];
	for (int i = 0;i < 3; ++i)
	{
		Backtracking<double>::convert_A2B(tri_A[i], rotation, translation, result[i]);
		CPPUNIT_ASSERT(abs(tri_B[i][0] - result[i][0]) < ERROR );
		CPPUNIT_ASSERT(abs(tri_B[i][1] - result[i][1]) < ERROR );
		CPPUNIT_ASSERT(abs(tri_B[i][2] - result[i][2]) < ERROR );
	}
	for (int i = 0; i< 3; ++i)
	{
		Backtracking<double>::convert_B2A(tri_B[i], rotation, translation, result [i]);
//		std::cout<<"From "<<tri_B[i]<<" to "<<result[i]<<std::endl;
		CPPUNIT_ASSERT(abs(tri_A[i][0] - result[i][0]) < ERROR );
		CPPUNIT_ASSERT(abs(tri_A[i][1] - result[i][1]) < ERROR );
		CPPUNIT_ASSERT(abs(tri_A[i][2] - result[i][2]) < ERROR );

	}

}


void Test_backtracking::test_intersection_01()
{
	SEGMENT seg1, seg2;
	seg1.begin = POINT (-1,0,0);
	seg1.end = POINT (1,0,0);

	seg2.begin = POINT (0, 1, 0);
	seg2.end   = POINT (0, -1, 0);

	POINT p;
	POINT result = POINT(0,0,0);
	
	CPPUNIT_ASSERT (true == Backtracking<double>::intersection(seg1, seg2, p));
	CPPUNIT_ASSERT ( (p - result ).length() < ERROR);
}
void Test_backtracking::test_intersection_02()
{
	SEGMENT seg1, seg2;
	seg1.begin = POINT (-1,0,0);
	seg1.end = POINT (1,0,0);

	seg2.begin = POINT (0, 0, 0);
	seg2.end   = POINT (0, -1, 0);

	POINT p;
	POINT result = POINT(0,0,0);
	CPPUNIT_ASSERT (true == Backtracking<double>::intersection(seg1, seg2, p));
	CPPUNIT_ASSERT ( (p - result ).length() < ERROR);
}
void Test_backtracking::test_intersection_03()
{
	SEGMENT seg1, seg2;
	seg1.begin = POINT (-1,0,0);
	seg1.end = POINT (1,0,0);

	seg2.begin = POINT (0, 1, 0);
	seg2.end   = POINT (0, 0.5, 0);

	POINT p;
	POINT result = POINT(0,0,0);
	CPPUNIT_ASSERT (false == Backtracking<double>::intersection(seg1, seg2, p));
}
void Test_backtracking::test_intersection_04()
{
	SEGMENT seg1, seg2;
	seg1.begin = POINT (-1,0,0);
	seg1.end = POINT (0,-1,0);

	seg2.begin = POINT (1, 1, 0);
	seg2.end   = POINT (0, 0, 0);

	POINT p;
	POINT result = POINT(0,0,0);
	CPPUNIT_ASSERT (false == Backtracking<double>::intersection(seg1, seg2, p));

}

void Test_backtracking::test_six_points_interpolation_01()
{
	const unsigned size = 6;
	POINT vertex[size];
	vertex[0] = POINT(0,0,0);  //A
	vertex[1] = POINT(1,0,0);  //B
	vertex[2] = POINT(1,1,0);  //C
	vertex[3] = POINT(0,-1,0); //D
	vertex[4] = POINT(2,1,0);  //E
	vertex[5] = POINT(0,1,0);  //F

	double value [] = {1, 1, 0, 2, 0, 0};
	POINT begin = POINT ( 0.5f, 0.0f, 0.0f);
	POINT end;
	int next_face_idx;
	CPPUNIT_ASSERT (true == Backtracking<double>::six_points_interpolation(begin, vertex, value, 0.01, end, next_face_idx));
	CPPUNIT_ASSERT ((end - POINT( 0.5,0.5,0)).length() < ERROR );

}
void Test_backtracking::test_six_points_interpolation_02()
{

}
void Test_backtracking::test_six_points_interpolation_03()
{
}
void Test_backtracking::test_six_points_interpolation_04()
{
}

void Test_backtracking::test_vertex_point_next_face_01()
{
	Mesh mesh;

	//bool result =  this->read_check_manifold_model(mesh, "../../data/Armadillo_20qem.obj");
	bool result =  this->read_check_manifold_model(mesh, "../../data/cossphere.obj");
	//bool result =  this->read_check_manifold_model(mesh, "../../data/plane5.obj");
	if (!result) {CPPUNIT_ASSERT_ASSERTION_FAIL("The model is invalid!"); return;}

	std::cerr<<"Fast Marching..."<<std::endl;
	//FMM starting point 
	int point_idx = 0; 
	this->vertex_point_fmm (mesh, point_idx);
	std::vector<Mesh::FaceHandle>  vec_end_face_handle ;
	for (Mesh::VertexFaceIter vf_it = mesh.vf_iter (mesh.vertex_handle(point_idx)); vf_it ; ++vf_it)
	{
		vec_end_face_handle.push_back(vf_it.handle());
	}
	Mesh::Point end = mesh.point(mesh.vertex_handle(point_idx));

	//This part is the test part 
	
	int test_vertex_idx = 25;
	//for (Mesh::VertexVertexIter vv_it = mesh.vv_iter(mesh.handle(test_vertex_idx)); vv_it; ++vv_it)
	//{
	//	std::cout<<"IDX      "<<vv_it.handle().idx() <<std::endl;
	//	std::cout<<"Distance "<<mesh.data(vv_it.handle()).get_distance()<<std::endl;
	//}
	//std::cout<<"---------------------------------------"<<std::endl;
	Mesh::FaceHandle next_face_handle;
	Backtracking<double>::vertex_point_next_face(mesh , mesh.vertex_handle(test_vertex_idx), next_face_handle, 0.001);
	
	for (Mesh::FaceVertexIter fv_it = mesh.fv_iter (next_face_handle); fv_it ; ++ fv_it)
	{
		//std::cout<<"IDX      "<<fv_it.handle().idx()<<std::endl;
		//std::cout<<"Distance "<<mesh.data(fv_it.handle()).get_distance()<<std::endl;
	}
	

}
void Test_backtracking::test_vertex_point_next_face_02()
{
}
void Test_backtracking::test_draw_geodesics_01()
{
	//This test case is testing the interfere of isolines. 
	Mesh mesh;	
	bool result =  this->read_check_manifold_model(mesh, "../../data/plane5.obj");
	//bool result =  this->read_check_manifold_model(mesh, "../../data/cossphere.obj");
	if (!result) {CPPUNIT_ASSERT_ASSERTION_FAIL("The model is invalid!"); return;}

	//this->face_centroid_fmm(mesh, 0);
	std::vector<int> point_idx_set;
	point_idx_set.push_back(0);
	point_idx_set.push_back(20);
	this->vertex_point_set_fmm (mesh, point_idx_set);
	unsigned n_vertices = mesh.n_vertices();

	Mesh::VertexIter v_it, v_end (mesh.vertices_end());
	std::vector<Mesh::Point> line;
	double max_dis = 0;
	for (v_it = mesh.vertices_begin(); v_it != v_end; ++v_it)
	{
//		std::cout<<mesh.data(v_it).get_distance()<<std::endl;;
		if (max_dis < mesh.data(v_it).get_distance())
		{
			max_dis = mesh.data(v_it).get_distance();
		}
	}
//	std::cout<<"MAX_DIS"<<max_dis<<std::endl;
	int n_lines = 100;
	double step = max_dis / (n_lines + 1);
	
	double isovalue = 0;
	for (int i = 0; i < n_lines; ++i)
	{
		isovalue +=step;		
		draw_one_isoline(mesh, isovalue, line);
	}
	
	assert (line.size() % 2 == 0); 
	std::ofstream outfile("./icossphere.obj");
	generate_seperate_mesh_isoline(mesh, line, outfile);
	
}
void Test_backtracking::test_draw_geodesics_01_2()
{
	Mesh mesh;

	//bool result =  this->read_check_manifold_model(mesh, "../../data/Armadillo_20qem.obj");
	bool result =  this->read_check_manifold_model(mesh, "../../data/cossphere.obj");
	//bool result =  this->read_check_manifold_model(mesh, "../../data/plane5.obj");
	if (!result) {CPPUNIT_ASSERT_ASSERTION_FAIL("The model is invalid!"); return;}

	std::cerr<<"Fast Marching..."<<std::endl;
	unsigned end_face_idx = 0; //760;

	Mesh::FaceHandle end_face = mesh.face_handle(end_face_idx);
	Mesh::Point end(0,0,0);
	for (Mesh::FaceVertexIter fv_it  = mesh.fv_iter(end_face); fv_it; ++fv_it)
	{
		
		end [0] +=mesh.point(fv_it)[0];
		end [1] +=mesh.point(fv_it)[1];
		end [2] +=mesh.point(fv_it)[2];
	}
	end [0] /= 3.0f;
	end [1] /= 3.0f;
	end [2] /= 3.0f;
	std::vector<Mesh::FaceHandle> vec_end_face_handle;
	vec_end_face_handle.push_back(end_face);

    //Where is the fast marching method? FIXME
	face_point_fmm (mesh, end_face.idx(), end);
	std::cerr<<"Draw Geodesics..."<<std::endl;

	unsigned n_vertices = mesh.n_vertices();

	Mesh::VertexIter v_it, v_end (mesh.vertices_end());
	std::vector<Mesh::Point> line;
	double max_dis = 0;
	for (v_it = mesh.vertices_begin(); v_it != v_end; ++v_it)
	{
//		std::cout<<mesh.data(v_it).get_distance()<<std::endl;;
		if (max_dis < mesh.data(v_it).get_distance())
		{
			max_dis = mesh.data(v_it).get_distance();
		}
	}
//	std::cout<<"MAX_DIS"<<max_dis<<std::endl;
	int n_lines = 100;
	double step = max_dis / (n_lines + 1);
	
	double isovalue = 0;
	for (int i = 0; i < n_lines; ++i)
	{
		isovalue +=step;		
		draw_one_isoline(mesh, isovalue, line);
	}
	
	assert (line.size() % 2 == 0); 
	std::ofstream outfile("./isofacecossphere.obj");
	//std::ofstream outfile("./isofaceplane5.obj");
	generate_seperate_mesh_isoline(mesh, line, outfile);
}
void Test_backtracking::test_draw_geodesics_02()
{
	Mesh mesh;

	//bool result =  this->read_check_manifold_model(mesh, "../../data/Armadillo_20qem.obj");
	bool result =  this->read_check_manifold_model(mesh, "../../data/cossphere.obj");
	//bool result =  this->read_check_manifold_model(mesh, "../../data/plane5.obj");
	if (!result) {CPPUNIT_ASSERT_ASSERTION_FAIL("The model is invalid!"); return;}

	std::cerr<<"Fast Marching..."<<std::endl;
	//FMM starting point 
	int point_idx = 0; 
	this->vertex_point_fmm (mesh, point_idx);
	std::vector<Mesh::FaceHandle>  vec_end_face_handle ;
	for (Mesh::VertexFaceIter vf_it = mesh.vf_iter (mesh.vertex_handle(point_idx)); vf_it ; ++vf_it)
	{
		vec_end_face_handle.push_back(vf_it.handle());
	}
	Mesh::Point end = mesh.point(mesh.vertex_handle(point_idx));


	std::cerr<<"Draw Geodesics..."<<std::endl;
	unsigned center_face_idx = 471; //760;

	Mesh::FaceHandle start_face = mesh.face_handle(center_face_idx);
	Mesh::Point start(0,0,0);
	for (Mesh::FaceVertexIter fv_it  = mesh.fv_iter(start_face); fv_it; ++fv_it)
	{
		
		start [0] +=mesh.point(fv_it)[0];
		start [1] +=mesh.point(fv_it)[1];
		start [2] +=mesh.point(fv_it)[2];
	}
	start [0] /= 3.0f;
	start [1] /= 3.0f;
	start [2] /= 3.0f;

	std::list<Mesh::Point> path;
	std::vector<Mesh::FaceHandle> face_record;
	Backtracking<double>::draw_geodesics(mesh,  start, start_face,  end, vec_end_face_handle, path, face_record, 0.001);
	std::cout<<"Path Size "<<path.size()<<std::endl;
	for (std::list<Mesh::Point>::iterator it = path.begin(); it != path.end(); ++it)
	{
		std::cout<<(*it)<<" | ";
	}
	std::cout<<std::endl;
	std::cout<<"Face Size "<<face_record.size()<<std::endl;
	for (int i = 0; i < face_record.size(); ++i)
	{
		std::cout<<face_record[i].idx()<<' ';
	}
	std::cout<<std::endl;
	face_record.clear();
	
	std::cerr<<"Generate Mesh..."<<std::endl;
	//std::ofstream outfile("./geoArmadillo_20qem.obj");
	std::ofstream outfile("./geocossphere.obj");
	generate_seperate_mesh_geodesics(mesh, path, outfile);

}
void Test_backtracking::test_draw_geodesics_03()
{

	Mesh mesh;

	//bool result =  this->read_check_manifold_model(mesh, "../../data/Armadillo_20qem.obj");
	bool result =  this->read_check_manifold_model(mesh, "../../data/cossphere.obj");
	//bool result =  this->read_check_manifold_model(mesh, "../../data/bar3.obj");
	//bool result =  this->read_check_manifold_model(mesh, "../../data/plane5.obj");
	if (!result) {CPPUNIT_ASSERT_ASSERTION_FAIL("The model is invalid!"); return;}

	std::cerr<<"Fast Marching..."<<std::endl;
	//FMM starting point 
	int point_idx = 0; 
	this->vertex_point_fmm (mesh, point_idx);
	std::vector<Mesh::FaceHandle>  vec_end_face_handle ;
	for (Mesh::VertexFaceIter vf_it = mesh.vf_iter (mesh.vertex_handle(point_idx)); vf_it ; ++vf_it)
	{
		vec_end_face_handle.push_back(vf_it.handle());
	}
	Mesh::Point end = mesh.point(mesh.vertex_handle(point_idx));


	std::cerr<<"Draw Geodesics..."<<std::endl;

	std::list<std::list<POINT> >  path_groups; 
	
	int face_count = 0;
	for (Mesh::FaceIter f_it=mesh.faces_begin(); f_it!=mesh.faces_end() ; ++f_it) 
	{
		
		
		Mesh::FaceHandle start_face = mesh.face_handle(face_count);
		Mesh::Point start(0,0,0);
		for (Mesh::FaceVertexIter fv_it  = mesh.fv_iter(start_face); fv_it; ++fv_it)
		{
		
			start [0] +=mesh.point(fv_it)[0];
			start [1] +=mesh.point(fv_it)[1];
			start [2] +=mesh.point(fv_it)[2];
		}
		start [0] /= 3.0f;
		start [1] /= 3.0f;
		start [2] /= 3.0f;

		std::list<Mesh::Point> path;
		std::vector<Mesh::FaceHandle> face_record;
		Backtracking<double>::draw_geodesics(mesh,  start, start_face,  end, vec_end_face_handle, path, face_record,  0.001);
		std::cerr<<"Face "<<face_count<<std::endl;
		path_groups.push_back(path);
		std::cout<<"Path Size : "<<path.size()<<" FaceRecord Size : "<<face_record.size()<<std::endl;
		path.clear();
		face_record.clear();
		face_count++;
	}
	std::cerr<<"Generate Mesh..."<<std::endl;
	//std::ofstream outfile("./geogpArmadillo_20qem.obj");
	std::ofstream outfile("./test_geodesics_03.obj");
	generate_seperate_group_geodesics(mesh, path_groups, outfile);	
}
void Test_backtracking::test_draw_geodesics_04()
{
	Mesh mesh;

	//bool result =  this->read_check_manifold_model(mesh, "../../data/Armadillo_20qem.obj");
	bool result =  this->read_check_manifold_model(mesh, "../../data/cossphere.obj");
	//bool result =  this->read_check_manifold_model(mesh, "../../data/plane5.obj");
	if (!result) {CPPUNIT_ASSERT_ASSERTION_FAIL("The model is invalid!"); return;}

	std::cerr<<"Fast Marching..."<<std::endl;
	unsigned end_face_idx = 5; //760;

	Mesh::FaceHandle end_face = mesh.face_handle(end_face_idx);
	Mesh::Point end(0,0,0);
	for (Mesh::FaceVertexIter fv_it  = mesh.fv_iter(end_face); fv_it; ++fv_it)
	{
		
		end [0] +=mesh.point(fv_it)[0];
		end [1] +=mesh.point(fv_it)[1];
		end [2] +=mesh.point(fv_it)[2];
	}
	end [0] /= 3.0f;
	end [1] /= 3.0f;
	end [2] /= 3.0f;
	std::vector<Mesh::FaceHandle> vec_end_face_handle;
	vec_end_face_handle.push_back(end_face);

    //Where is the fast marching method? FIXME
	face_point_fmm (mesh, end_face.idx(), end);
	std::cerr<<"Draw Geodesics..."<<std::endl;

	std::list<std::list<POINT> >  path_groups; 
	int face_count_idx = 0;

//	int face_count [] = {1272,1267};
	for (Mesh::FaceIter f_it=mesh.faces_begin(); f_it!=mesh.faces_end() ; ++f_it) 
	{
		if (end_face_idx == face_count_idx) 
		{
			face_count_idx ++;
			continue;
		}
		
		
		std::cout<<face_count_idx<<std::endl;

		Mesh::FaceHandle start_face = mesh.face_handle(face_count_idx);
		Mesh::Point start(0,0,0);
		for (Mesh::FaceVertexIter fv_it  = mesh.fv_iter(start_face); fv_it; ++fv_it)
		{
		
			start [0] +=mesh.point(fv_it)[0];
			start [1] +=mesh.point(fv_it)[1];
			start [2] +=mesh.point(fv_it)[2];
		}
		start [0] /= 3.0f;
		start [1] /= 3.0f;
		start [2] /= 3.0f;

		std::list<Mesh::Point> path;
		std::vector<Mesh::FaceHandle> face_record;
		Backtracking<double>::draw_geodesics(mesh,  start, start_face,  end, vec_end_face_handle, path, face_record,  0.001);
		face_record.clear();
		path_groups.push_back(path);
		path.clear();
		face_count_idx ++;
	}
	std::cerr<<"Generate Mesh..."<<std::endl;
	//std::ofstream outfile("./geoArmadillo_20qem.obj");
	//std::ofstream outfile("./geocossphere.obj");
	//generate_seperate_mesh_geodesics(mesh, path, outfile);
	std::ofstream outfile("./Test_geodesics_04.obj");
	generate_seperate_group_geodesics(mesh, path_groups, outfile);	
}
